{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Inverse kinematics\n",
    "This notebook considers the problem of inverse kinematics, ie solving at each control cycle of the robot a quadratic program from the derivatives (jacobian) of the current state. It introduces the basic function to compute the Jacobians of the robot, and how to use them to compute an inverse-kinematics control law. One of the key difficulties is to understand in which frames each quantities is computed (might be in the world frame, in the local frame attached to the end effector, in some arbitrary goal frame, etc), as we should never mixed quantities expressed in different frames without transporting them in the right frame. \n",
    "\n",
    "本笔记本探讨逆运动学问题，即在机器人的每个控制周期，根据当前状态的导数（雅可比矩阵）求解一个二次规划问题。它介绍了计算机器人雅可比矩阵的基本函数，以及如何利用这些函数来计算逆运动学控制律。其中一个关键难点在于理解每个量是在哪个坐标系中计算的（可能是在世界坐标系、与末端执行器相连的局部坐标系、某个任意目标坐标系等），因为我们绝不能在未将不同坐标系中表示的量转换到正确坐标系的情况下就将它们混用。 \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "import magic_donotload  # noqa: F401"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Set up\n",
    "We will MeshCat for visualization and NumPy for linear algebra. Note that the [@ operator](https://numpy.org/doc/stable/reference/routines.linalg.html#the-operator) is a way to compute the matrix (dot) product between two NumPy arrays, similar to ``np.dot(A, B)``.\n",
    "\n",
    "我们将使用MeshCat进行可视化，使用NumPy进行线性代数计算。请注意，[@运算符](https://numpy.org/doc/stable/reference/routines.linalg.html#the-operator) 是一种计算两个NumPy数组之间矩阵（点）积的方法，类似于 ``np.dot(A, B)``。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "import pinocchio as pin\n",
    "import numpy as np\n",
    "import time\n",
    "from numpy.linalg import pinv,inv,norm,svd,eig\n",
    "from tp3.tiago_loader import loadTiago\n",
    "import matplotlib.pylab as plt; plt.ion()\n",
    "from utils.meshcat_viewer_wrapper import MeshcatVisualizer"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will use the [Tiago](https://youtu.be/6BwRqwD066g) robot, a mobile manipulator from PAL Robotics:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7001/static/\n"
     ]
    }
   ],
   "source": [
    "robot = loadTiago()\n",
    "viz = MeshcatVisualizer(robot)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Click on the link above to open the viewer in a separate window, or execute the following cell to embed the viewer here:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "            <div style=\"height: 400px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
       "            <iframe src=\"http://127.0.0.1:7001/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "            </div>\n",
       "            "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 25,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Tiago consists of:\n",
    "\n",
    "- A mobile base that can move in the plane (the corresponding joint will be a *floating base* in $SE(2)$, thus with three degrees of freedom).\n",
    "- A manipulator arm (seven DOFs).\n",
    "- An articulated head (two DOFs).\n",
    "- A prismatic axis moving vertically (one DOF), on which arm and head are mounted.\n",
    "- Two extra joints to represent the wheels under the mobile base (two DOFs), which we won't use in this tutorial.\n",
    "\n",
    "- 一个可在平面内移动的移动基座（相应的关节在$SE(2)$中为*浮动基座*，因此具有三个自由度）。\n",
    "- 一个机械臂（七个自由度）。\n",
    "- 一个可活动的头部（两个自由度）。\n",
    "- 一个垂直移动的棱柱轴（一个自由度），机械臂和头部安装在该轴上。\n",
    "- 两个额外的关节，用于表示移动基座下方的轮子（两个自由度），本教程中不会使用。\n",
    "\n",
    "The wheel and the base rotations are represented by the vector $[\\cos \\theta\\ \\sin \\theta]$ corresponding to their angle $\\theta$. This choice of representation results in a larger vector, subject to the constraint $\\cos(\\theta)^2 + \\sin(\\theta)^2 = 1$, but with no discontinuity at the boundary of the angular interval chosen to select a unique angle. The dimension of the configuration space (equivalently, the size of the configuration vector) is then:\n",
    "\n",
    "轮子和底座的旋转由向量 $[\\cos \\theta\\ \\sin \\theta]$ 表示，该向量对应于它们的角度 $\\theta$。这种表示选择会产生一个更大的向量，需满足约束条件 $\\cos(\\theta)^2 + \\sin(\\theta)^2 = 1$，但在为选择唯一角度而选定的角度区间边界处不存在不连续性。那么，配置空间的维度（等效于配置向量的大小）为：\n",
    "\n",
    "| Joint          | DOFs | Dim. of config. repr. | Dim. of tangent repr. |\n",
    "|----------------|------|-----------------------|-----------------------|\n",
    "| Mobile base    | 3    | 4  | 3 |\n",
    "| Manipulator    | 7    | 7  | 7 |\n",
    "| Head           | 2    | 2  | 2 |\n",
    "| Prismatic axis | 1    | 1  | 1 |\n",
    "| Wheels         | 2    | 4  | 2 |\n",
    "| **Total**      | **15** | **18** | **15** |\n",
    "\n",
    "You can check these values in ``robot.model.nq`` and ``robot.model.nv``.\n",
    "\n",
    "The configuration is represented by a vector of larger dimension, subject to constraints $\\cos(\\theta)^2 + \\sin(\\theta)^2 = 1$. It is not possible to randomly sample a configuration vector $q$ using the distributions from NumPy, as the results won't respect these constraints. Similarly, we should take care when integrating velocities, as summing a configuration $q$ with a velocity $v$ is undefined (dimensions don't match). Two functions in Pinocchio implements these functionnalities.\n",
    "\n",
    "该配置由一个更高维度的向量表示，并受限于约束条件 $\\cos(\\theta)^2 + \\sin(\\theta)^2 = 1$。由于结果不会满足这些约束条件，因此无法使用NumPy中的分布随机采样配置向量 $q$。同样，在对速度进行积分时我们也应注意，因为将配置 $q$ 与速度 $v$ 相加是没有定义的（维度不匹配）。Pinocchio中的两个函数实现了这些功能。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = pin.randomConfiguration(robot.model)\n",
    "vq = np.random.rand(robot.model.nv)*2 - 1\n",
    "DT = 1e-3\n",
    "qnext = pin.integrate(robot.model,q,vq*DT)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(array([-0.21704334,  0.53253163,  0.33572277, -0.41101734,  0.1879101 ,\n",
       "         0.51315726, -0.43967334, -0.75618385,  0.40075235,  0.39304576,\n",
       "         0.24691051,  0.71571611, -0.23278429,  0.19593379,  0.02304823]),\n",
       " 15)"
      ]
     },
     "execution_count": 27,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "vq, len(vq)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "len_q:18 robot.model.nv:15 robot.model.nq:15 \n",
      "q:[-0.49035262 -0.14427582 -0.85433672 -0.51971989  0.11997963  2.19366411\n",
      "  0.98846119  0.19431288  0.8319536   1.32806651  1.14186077  0.01459349\n",
      "  1.12134448 -0.0191981   0.98772483 -0.1562039   0.97193858  0.23523475]  \n",
      "vq:[-0.21704334  0.53253163  0.33572277 -0.41101734  0.1879101   0.51315726\n",
      " -0.43967334 -0.75618385  0.40075235  0.39304576  0.24691051  0.71571611\n",
      " -0.23278429  0.19593379  0.02304823]\n"
     ]
    }
   ],
   "source": [
    "print(f\"len_q:{len(q)} robot.model.nv:{robot.model.nv} robot.model.nq:{robot.model.nv} \\nq:{q}  \\nvq:{vq}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is a simple example where we move the robot in the viewer following a constant (random) velocity:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [],
   "source": [
    "for t in range(1000):\n",
    "    q = pin.integrate(robot.model,q,vq*DT)\n",
    "    viz.display(q)\n",
    "    time.sleep(DT/10)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The robot is mobile, hence the camera view in the viewer is not always centered. Also, don't worry if you see the torso fly above the prismatic joint: we are not including joint limits yet 😉\n",
    "\n",
    "该机器人是可移动的，因此查看器中的相机视图并不总是居中。另外，如果你看到躯干飞到棱柱形关节上方也不用担心：我们目前还未纳入关节限制。 "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Forward kinematics and Jacobian\n",
    "\n",
    "Here's a quick recap on forward kinematics. Let's consider two frames of interest: the first one, named \"frametool\" is at the tip of the end-effector; the second one, named \"framebasis\", is at the fore of the robot's mobile base, 10 cm above the ground. Both are represented in MeshCat by a triad of RGB arrows:\n",
    "\n",
    "以下是对正向运动学的快速回顾。我们考虑两个感兴趣的坐标系：第一个名为“frametool”，位于末端执行器的尖端；第二个名为“framebasis”，位于机器人移动基座的前端，距离地面10厘米。在MeshCat中，这两个坐标系都由一组RGB箭头组成的三元组表示："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [],
   "source": [
    "IDX_TOOL = robot.model.getFrameId('frametool')\n",
    "IDX_BASIS = robot.model.getFrameId('framebasis')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Frame name: frametool paired to (parent joint/ previous frame)(9/52)\n",
       "with relative placement wrt parent joint:\n",
       "  R =\n",
       "1 0 0\n",
       "0 1 0\n",
       "0 0 1\n",
       "  p =    0    0 0.08\n",
       "containing inertia:\n",
       "  m = 0\n",
       "  c = 0 0 0\n",
       "  I = \n",
       "0 0 0\n",
       "0 0 0\n",
       "0 0 0"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.model.frames[IDX_TOOL]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The `frames` vector in a `robot.model` contains relative placements in $SE(3)$: ``frames[i]`` gives the placement from frame $i$ to its parent frame in the kinematic tree. If the frame is that of a link rather than a joint, it will also contain non-zero inertias.\n",
    "\n",
    "在“robot.model”中的“frames”向量包含 $SE(3)$ 中的相对位姿：“frames[i]”给出了运动学树中从第 $i$ 帧到其父帧的位姿。如果该帧是连杆的帧而不是关节的帧，它还将包含非零惯性。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Computing frame placement\n",
    "\n",
    "The global placement of the frames is computed by the Pinocchio function `framesForwardKinematics`, whose results are stored in `robot.data.oMf`. Read `oMf` as ${}^0 M_f$, where $0$ is the index of our inertial frame (*a.k.a.* the world frame).\n",
    "\n",
    "框架的全局位姿由Pinocchio函数`framesForwardKinematics`计算得出，其结果存储在`robot.data.oMf`中。将`oMf`理解为${}^0 M_f$，其中$0$是我们惯性系（又称世界坐标系）的索引。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Tool placement:   R =\n",
      "-0.275412 -0.822143 -0.498227\n",
      "0.0345509 -0.526401  0.849534\n",
      "-0.960705  0.216758  0.173383\n",
      "  p = -0.0241767  -0.603065    1.05037\n",
      "\n",
      "Basis placement:   R =\n",
      "-0.635419  0.772168         0\n",
      "-0.772168 -0.635419         0\n",
      "        0         0         1\n",
      "  p = -0.170518 -0.634836      0.15\n",
      "\n"
     ]
    }
   ],
   "source": [
    "pin.framesForwardKinematics(robot.model, robot.data, q)\n",
    "\n",
    "oMtool = robot.data.oMf[IDX_TOOL]\n",
    "oMbasis = robot.data.oMf[IDX_BASIS]\n",
    "\n",
    "print(\"Tool placement:\",oMtool)\n",
    "print(\"Basis placement:\",oMbasis)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It is important to note the instruction pattern, which is standard in all Pinocchion functions: first, call an algorithm (here ``pin.framesForwardKinematics``); then, access the results in *robot.data*. Our `robot.model` contains the abstract structure (the model) of our robot, whereas `robot.data` contains a particular instance of that structure, populated by the algorithm for a given instance of configuration, velocity, etc.\n",
    "\n",
    "需要注意的是指令模式，这在所有Pinocchion函数中都是标准的：首先，调用一个算法（这里是 ``pin.framesForwardKinematics``）；然后，在 *robot.data* 中访问结果。我们的 `robot.model` 包含机器人的抽象结构（模型），而 `robot.data` 包含该结构的一个特定实例，由针对给定配置、速度等实例的算法填充。 "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The tool-placement matrix $^0M_{tool}$ represents the displacement between the tool frame $F_{tool}$ and the world frame $F_{world}$. It is the affine transform matrix that maps vectors expressed in $F_{tool}$ to their corresponding vectors expressed in $F_0$. It consists of a $3 \\times 3$ rotation matrix $^0 R_{tool}$, and a 3D position vector:\n",
    "\n",
    "刀具位姿矩阵$^0M_{tool}$表示刀具坐标系$F_{tool}$与世界坐标系$F_{world}$之间的位移。它是一个仿射变换矩阵，用于将在$F_{tool}$中表示的向量映射到在$F_0$中表示的对应向量。它由一个$3 \\times 3$的旋转矩阵$^0 R_{tool}$和一个三维位置向量组成：\n",
    "\n",
    "$$\n",
    "^0T_{tool} = {}^0 (p_{tool} - p_{0}) = {}^0 p_{tool} - {}^0 p_{0} = {}^0 p_{tool}.\n",
    "$$\n",
    "\n",
    "This is the vector from the origin of frame $F_0$ to the origin of frame $F_{tool}$, expressed in the world frame $F_0$. We can also express $p_{tool} - p_{0}$ in the tool frame $F_{tool}$ by multiplying $^0T_{tool}$ by ${}^{tool} R _0 = {}^{0}R_{tool}^T$:\n",
    "\n",
    "这是从坐标系$F_0$的原点到工具坐标系$F_{tool}$原点的向量，在世界坐标系$F_0$中表示。我们也可以通过将$^0T_{tool}$与${}^{tool} R _0 = {}^{0}R_{tool}^T$相乘，在工具坐标系$F_{tool}$中表示$p_{tool} - p_{0}$：\n",
    "\n",
    "$$\n",
    "{}^{tool} (p_{tool} - p_0) = - {}^{tool}p_0 = R_0 {}^0 T_{tool}.\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Take care with the multiplication operator in NumPy: the operator ``*`` is (unintuitively) mapped to the coefficient-wise multiplication (*a.k.a.* the [Hadamard product](Hadamard product)), that is, **not** the usual matrix multiplication. You should use the operator ``@`` to get an actual matrix-to-matrix dot product. (Or never mind: you will likely be tricked at least once by this, then remember it out of spite 😅) \n",
    "\n",
    "在NumPy中使用乘法运算符时要注意：运算符 ``*`` （反直觉地）映射到逐元素乘法（也称为[哈达玛积](Hadamard product)），也就是说，**不是** 通常的矩阵乘法。你应该使用运算符 ``@`` 来进行实际的矩阵与矩阵的点积运算。（或者没关系：你很可能至少会被这个问题坑一次，然后出于怨恨记住它 😅） "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Computing Jacobians\n",
    "The Jacobian of a frame of the robot is computed using `pin.computeFrameJacobian`:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(6, 15)"
      ]
     },
     "execution_count": 34,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Jtool = pin.computeFrameJacobian(robot.model, robot.data, q, IDX_TOOL)\n",
    "Jtool.shape"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This matrix has 6 rows and ``robot.model.nv == 15`` columns. It corresponds to the \"body\" 6D velocity of the end effector, that is, vectors in the image of this Jacobian matrix are expressed in $F_{tool}$ rather than in $F_0$. Let's first focus on the first three rows. Those correspond to the linear velocity of the tool frame, in the tool frame:\n",
    "\n",
    "这个矩阵有6行，且“robot.model.nv == 15”列。它对应于末端执行器的“主体”6D速度，也就是说，这个雅可比矩阵值域中的向量是在$F_{tool}$中表示的，而非在$F_0$中。我们先关注前三行。这三行对应于工具坐标系下工具框架的线速度："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.14832277, -0.23461848, -0.0565793 , -0.96070522, -0.03267153,\n",
       "         0.04911019, -0.01346038, -0.09349446,  0.02066938,  0.07728018,\n",
       "         0.        ,  0.        ,  0.        ,  0.        ,  0.        ],\n",
       "       [ 0.92887436, -0.30034733, -0.14101857,  0.2167577 , -0.10006701,\n",
       "        -0.26943864, -0.05449739,  0.07631586,  0.07723041, -0.0206827 ,\n",
       "         0.        ,  0.        ,  0.        ,  0.        ,  0.        ],\n",
       "       [-0.33940061, -0.92452455, -0.13720607,  0.17338274, -0.05593067,\n",
       "         0.57283492,  0.00372322, -0.30719655,  0.        ,  0.        ,\n",
       "         0.        ,  0.        ,  0.        ,  0.        ,  0.        ]])"
      ]
     },
     "execution_count": 36,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Jtool3 = Jtool[:3,:]\n",
    "Jtool3"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Jacobian as a velocity operator\n",
    "A first way to understand what is this matrix is to see that as an operator that converts the velocity in the configuration space to the linear velocity of the tool:\n",
    "\n",
    "理解这个矩阵的第一种方法是将其视为一个算子，它将构型空间中的速度转换为工具的线速度："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "vtool = Jtool3 @ vq"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "But in which frame is $v_{tool}$ expressed? The choice in Pinocchio (following the principles described in [Rigid body dynamics algorithms](https://doi.org/10.1007/978-1-4899-7560-7)) is to express quantities in the local frame by default. Therefore, ``vtool`` here is ${}^{tool} v_{tool}$, expressed in the tool frame $F_{tool}$. We can compute the velocity in the world frame $F_0$ by applying the rotation matrix $^{0}R_{tool}$:\n",
    "\n",
    "但$v_{tool}$是在哪个坐标系中表示的呢？在Pinocchio中（遵循[刚体动力学算法](https://doi.org/10.1007/978-1-4899-7560-7)中描述的原则），默认选择在局部坐标系中表示量。因此，这里的“vtool”是${}^{tool} v_{tool}$，在工具坐标系$F_{tool}$中表示。我们可以通过应用旋转矩阵$^{0}R_{tool}$来计算世界坐标系$F_0$中的速度："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "tool_vtool = vtool\n",
    "o_vtool = oMtool.rotation @ vtool"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The resulting velocity, that we denote by ${}^0 v_{tool}$ here, is ${}^{tool} v_{tool}$ rotated to the world frame."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 38,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.14832277, -0.23461848, -0.0565793 , -0.96070522, -0.03267153,\n",
       "         0.04911019, -0.01346038, -0.09349446,  0.02066938,  0.07728018,\n",
       "         0.        ,  0.        ,  0.        ,  0.        ,  0.        ],\n",
       "       [ 0.92887436, -0.30034733, -0.14101857,  0.2167577 , -0.10006701,\n",
       "        -0.26943864, -0.05449739,  0.07631586,  0.07723041, -0.0206827 ,\n",
       "         0.        ,  0.        ,  0.        ,  0.        ,  0.        ],\n",
       "       [-0.33940061, -0.92452455, -0.13720607,  0.17338274, -0.05593067,\n",
       "         0.57283492,  0.00372322, -0.30719655,  0.        ,  0.        ,\n",
       "         0.        ,  0.        ,  0.        ,  0.        ,  0.        ]])"
      ]
     },
     "execution_count": 38,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "tool_Jtool = pin.computeFrameJacobian(robot.model, robot.data, q, IDX_TOOL, pin.LOCAL)\n",
    "tool_Jtool3 = tool_Jtool[:3,:]\n",
    "tool_Jtool3"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We are using long variable names here to keep track. (A common practice is to only shorten these variable names when their full meaning is clear from context.) We can generalize this notation to the Jacobian part dedidcated to linear velocity expressed in the world frame $F_0$:\n",
    "\n",
    "我们在这里使用较长的变量名以便跟踪。（一种常见做法是，只有当变量的完整含义从上下文清晰可知时，才缩短这些变量名。）我们可以将这种表示法推广到专门用于在世界坐标系$F_0$中表示的线速度的雅可比矩阵部分："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "o_Jtool3 = oMtool.rotation @ tool_Jtool3"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To be more precise, the relation between Jacobian, motion and frame is:\n",
    "$$\n",
    "^{tool}J_{tool} \\dot{q} = ^{tool}\\xi_{tool} = \\begin{bmatrix} ^{tool}v_{tool} \\\\ ^{tool}\\omega_{tool} \\end{bmatrix}\n",
    "$$\n",
    "Here ${}^{tool} \\xi_{tool}$ is the *twist* (the instantaneous rigid-body motion, including both linear and angular velocity) from frame $F_{tool}$ to frame $F_0$, expressed in $F_{tool}$ itself. Mathematically, twist is just a name for any element of the Lie algebra $\\mathfrak{se}(3)$. We can interpret $^{tool}v_{tool}$ as the point velocity of the origin of frame $F_{tool}$, expressed in itself, and $^{tool}\\omega_{tool}$ as the instaneous rotation axis/amplitude.\n",
    "\n",
    "这里 ${}^{tool} \\xi_{tool}$ 是 *扭转*（瞬时刚体运动，包括线速度和角速度），从坐标系 $F_{tool}$ 到坐标系 $F_0$，在坐标系 $F_{tool}$ 自身中表示。从数学角度讲，扭转只是李代数 $\\mathfrak{se}(3)$ 中任何元素的一个名称。我们可以将 $^{tool}v_{tool}$ 解释为坐标系 $F_{tool}$ 原点的点速度，在其自身中表示，而 $^{tool}\\omega_{tool}$ 解释为瞬时旋转轴/幅度。 "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Adjoint operator\n",
    "\n",
    "We can also compute the twist ${}^0 w_{tool}$ in the world frame. Its formula is given by:\n",
    "$$\n",
    "{}^{0}\\xi_{tool} = {}^{0}J_{tool}\\dot{q} = {}^{0}X_{tool}{}^{tool}\\xi_{tool} = ({}^{0}X_{tool}{}^{tool}J_{tool}) \\dot{q}\n",
    "$$\n",
    "where $^{0}X_{tool}$ is the *adjoint operator* related to $[{}^{0}R_{tool}, {}^{0}T_{tool}]$ that allows us to change frame for elements of $\\mathfrak{se}(3)$.\n",
    "It express:\n",
    "$$\n",
    "{}^{0}X_{tool} = \\begin{bmatrix} ^{0}R_{tool} &  [{}^{0}T_{tool}]_\\times {}^{0}R_{tool} \\\\\n",
    "0 & {}^{0}R_{tool} \\end{bmatrix}\n",
    "$$\n",
    "Recall that $^{tool}\\xi_{tool} = [^{tool}v_{tool}, ^{tool}\\omega_{tool}]$. We calculate:\n",
    "$$\n",
    "^{0}\\xi_{tool} =  {}^{0}X_{tool}{}^{tool}\\xi_{tool} = [{}^{0}R_{tool}{}^{tool}v_{tool} + {}^{0}T_{tool} \\times {}^{0}R_{tool}{}^{tool}\\omega_{tool},  {}^0R_{tool}{}^{tool}\\omega_{tool}]\n",
    "$$\n",
    "And we have:\n",
    "$$\n",
    "^{0}\\xi_{tool}=[^0v_{tool} + {}^0T_{tool} \\times {}^{0}\\omega_{tool}, ^{0}\\omega_{tool}]\n",
    "$$\n",
    "Note here that the angular component of $^{0}\\xi_{tool}$ is indeed $^{0}\\omega_{tool}$, but the linear part of $^{0}\\xi_{tool}$ is **not** the vector $^0v_{tool}$ from above. Instead, the Varignon formula applies. The linear part of $^{0}\\xi_{tool}$ does not have an easy interpretation on its own: it corresponds to the instantaneous velocity of a point of the tool frame that coincides with the origin of the world frame, expressed in the world frame.\n",
    "\n",
    "请注意，这里$^{0}\\xi_{tool}$的角向分量确实是$^{0}\\omega_{tool}$，但$^{0}\\xi_{tool}$的线性部分**并非**上述的向量$^0v_{tool}$。相反，这里适用伐里农公式。$^{0}\\xi_{tool}$的线性部分本身并没有一个简单的物理解释：它对应于刀具坐标系中与世界坐标系原点重合的一点的瞬时速度，该速度是在世界坐标系中表示的。 "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Jacobian as a derivative of frame placements\n",
    "\n",
    "Previously, we saw Jacobians as operators transforming configuration velocities into twists. A second interpretation of is to observe that it is the derivative of the placement following $q$. For the three first colums, it is the derivative of the vector $^0T_{tool} = {}^0(o_{tool} - o_0)$ the tool position in world frame. Indeed $^0T_{tool}$ is a function of $q$, $^0T_{tool}(q)$ in full writing. We can take its derivative with respect to $q$, itself express in the world frame,  denoted ${}^{0}\\frac{\\partial {}^{0}T_{tool}}{\\partial q}$. This derivative is equal to the Jacobian expressed in the world frame $F_0$:\n",
    "\n",
    "此前，我们将雅可比矩阵视为将构型速度转换为扭转的算子。另一种解释是，它是随 $q$ 变化的位姿的导数。对于前三个列向量，它是向量 $^0T_{tool} = {}^0(o_{tool} - o_0)$（工具在世界坐标系中的位置）的导数。实际上，$^0T_{tool}$ 是 $q$ 的函数，完整写法为 $^0T_{tool}(q)$。我们可以对其关于 $q$ 求导，$q$ 本身在世界坐标系中表示，记为 ${}^{0}\\frac{\\partial {}^{0}T_{tool}}{\\partial q}$。这个导数等于在世界坐标系 $F_0$ 中表示的雅可比矩阵：\n",
    "$$\n",
    "\\vphantom{\\frac{\\partial}{\\partial}}^{0}\\frac{\\partial ^0T_{tool}}{\\partial q} = {}^{0}J_{tool}^{(3)}.\n",
    "$$\n",
    "The global Jacobian is the same for the whole motion expression, using the algebra of the Lie group $SE(3)$:\n",
    "\n",
    "$$\n",
    "^{tool}J_{tool}(q) u = \\vphantom{\\frac{\\partial}{\\partial}}^{tool}\\frac{\\partial {}^0M_{tool}}{\\partial q} u = \\lim_{t \\to 0^+}\\frac{^{0}M_{tool}(q \\oplus_{\\mathcal{C}} tu) \\ominus_{SE(3)} \\vphantom{.}^{0}M_{tool}(q)}{t}\n",
    "$$\n",
    "\n",
    "where the $\\oplus_{\\mathcal{C}}$ operator is the exponential on the configuration space $\\mathcal{C}$, implemented for us by Pinocchio's `pin.integrate(model, q, dq)` function. You can think of it by analogy with $SE(3)$:\n",
    "\n",
    "$$\n",
    "{}^{0} M_{tool} \\oplus_{SE(3)} {}^{tool} \\xi_{tool} = {}^0 M_{tool} \\exp({}^{tool} \\xi_{tool})\n",
    "$$\n",
    "\n",
    "The minus operator $\\ominus_{SE(3)}$ corresponds to the logarithm:\n",
    "\n",
    "$$\n",
    "{}^{0} M_{goal} \\ominus_{SE(3)} {}^0 M_{tool} = \\log({}^0 M_{tool}^{-1} {}^0 M_{goal}) = \\log({}^{tool} M_{goal})\n",
    "$$\n",
    "\n",
    "In this expression, the resulting vector $\\log({}^{tool} M_{goal})$ belongs to the tool frame $F_{tool}$ (informally, $\\log({}^b M_a)$ is in frame $b$). In Lie groups, be it $SE(3)$ to describe rigid-body transformations, or $\\mathcal{C}$ the configuration space of our robot, things don't commute. If we had taken ${}^{0} M_{tool} \\ominus_{SE(3)} {}^{0} M_{goal}$ instead, we would have obtained a twist in the goal frame.\n",
    "\n",
    "To convince ourselves of these observations, let's check finite differences. We can take a small displacement $\\delta q$, and check that the change in position ${}^0 T_{tool}(q + \\delta q)$ matches the linear prediction ${}^{0}J_{tool}^{(3)} \\delta q$:\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Full Jacobian in the tool frame:\n",
      "With a Jac: [ 0.51859583 -0.61157792 -0.87365518 -1.08129646 -0.56305816 -0.78646518]\n",
      "With a log: [ 0.51861558 -0.61153649 -0.87369432 -1.08127755 -0.56308693 -0.78648842]\n",
      "\n",
      "Origin velocity in the world frame:\n"
     ]
    },
    {
     "ename": "NameError",
     "evalue": "name 'o_Jtool3' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[39], line 24\u001b[0m\n\u001b[1;32m     21\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mWith a log: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mpin\u001b[38;5;241m.\u001b[39mlog(o_M_tool\u001b[38;5;241m.\u001b[39minverse()\u001b[38;5;250m \u001b[39m\u001b[38;5;241m*\u001b[39m\u001b[38;5;250m \u001b[39mo_M_tool2)\u001b[38;5;241m.\u001b[39mvector\u001b[38;5;250m \u001b[39m\u001b[38;5;241m/\u001b[39m\u001b[38;5;250m \u001b[39mEPS\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[1;32m     23\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mOrigin velocity in the world frame:\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m---> 24\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mWith a Jac: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00m\u001b[43mo_Jtool3\u001b[49m\u001b[38;5;250m \u001b[39m\u001b[38;5;241m@\u001b[39m\u001b[38;5;250m \u001b[39mu\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[1;32m     25\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mWith a log: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mo_M_tool\u001b[38;5;241m.\u001b[39mrotation\u001b[38;5;250m \u001b[39m\u001b[38;5;241m@\u001b[39m\u001b[38;5;250m \u001b[39mpin\u001b[38;5;241m.\u001b[39mlog(o_M_tool\u001b[38;5;241m.\u001b[39minverse()\u001b[38;5;250m \u001b[39m\u001b[38;5;241m*\u001b[39m\u001b[38;5;250m \u001b[39mo_M_tool2)\u001b[38;5;241m.\u001b[39mlinear\u001b[38;5;250m \u001b[39m\u001b[38;5;241m/\u001b[39m\u001b[38;5;250m \u001b[39mEPS\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[1;32m     26\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mWith fdiff: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00m(o_T_tool2\u001b[38;5;250m \u001b[39m\u001b[38;5;241m-\u001b[39m\u001b[38;5;250m \u001b[39mo_T_tool)\u001b[38;5;250m \u001b[39m\u001b[38;5;241m/\u001b[39m\u001b[38;5;250m \u001b[39mEPS\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n",
      "\u001b[0;31mNameError\u001b[0m: name 'o_Jtool3' is not defined"
     ]
    }
   ],
   "source": [
    "# Sample between -0.001 and 0.001\n",
    "EPS = 1e-4\n",
    "u = (2 * np.random.rand(robot.model.nv) - 1)\n",
    "dq = u*EPS\n",
    "\n",
    "# q2 = q+dq\n",
    "q2 = pin.integrate(robot.model, q, dq)\n",
    "\n",
    "# tool position for q\n",
    "pin.framesForwardKinematics(robot.model,robot.data,q)\n",
    "o_M_tool = robot.data.oMf[IDX_TOOL].copy()\n",
    "o_T_tool = o_M_tool.translation\n",
    "\n",
    "# tool position for q+dq\n",
    "pin.framesForwardKinematics(robot.model,robot.data,q2)\n",
    "o_M_tool2 = robot.data.oMf[IDX_TOOL].copy()\n",
    "o_T_tool2 = o_M_tool2.translation\n",
    "\n",
    "print(\"Full Jacobian in the tool frame:\")\n",
    "print(f\"With a Jac: {tool_Jtool @ u}\")\n",
    "print(f\"With a log: {pin.log(o_M_tool.inverse() * o_M_tool2).vector / EPS}\")\n",
    "\n",
    "print(\"\\nOrigin velocity in the world frame:\")\n",
    "print(f\"With a Jac: {o_Jtool3 @ u}\")\n",
    "print(f\"With a log: {o_M_tool.rotation @ pin.log(o_M_tool.inverse() * o_M_tool2).linear / EPS}\")\n",
    "print(f\"With fdiff: {(o_T_tool2 - o_T_tool) / EPS}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Frame options in Pinocchio\n",
    "\n",
    "Most algorithms accept an option to specify in wich frame the spatial quantity should be expressed. The two basic options are `pin.LOCAL` and `pin.WORLD`. When related to velocity, *LOCAL* is the linear velocity of the center of the local frame (the TOOL_IDX frame, here) and the angular velocity, both expressed in the local frame. With *WORLD* frame, this is the instantaneous motion expressed in the world frame. It is also compose of a linear and an angular velocity but remember that the linear velocity is then difficult to interpret.\n",
    "\n",
    "A last option is given by convenience, which does not respect the mathematics of spatial velocity: `pin.LOCAL_WORLD_ALIGNED` gives the linear velocity of the center of the local frame and the angular velocity, both expressed in the world frame. It is convenient especially when we are interested to consider the linear velocity as the derivative of the position. This is what we did above.\n",
    "\n",
    "To recap:\n",
    "- `pin.LOCAL` gives $^{tool}\\xi_{tool} = [^{tool}v_{tool}, ^{tool}\\omega_{tool}]$\n",
    "- `pin.WORLD` gives $^{0}\\xi_{tool} =\\vphantom{.}^{0}X_{tool}\\vphantom{.}^{tool}\\xi_{tool}=\\left(\\begin{array}{c|c} \n",
    "  ^0R_{tool} & [^0T_{tool}]_{\\times}\\vphantom{.}^0R_{tool} \\\\ \n",
    "  \\hline \n",
    "  0 & ^0R_{tool}\n",
    "\\end{array} \n",
    "\\right)\\vphantom{.}^{tool}\\xi_{tool} = [^0v_{tool} + {}^0T_{tool} \\times {}^{0}\\omega_{tool}, ^{0}\\omega_{tool}]$\n",
    "- `pin.LOCAL_WORLD_ALIGNED` gives $[^{0}v_{tool}, ^{0}\\omega_{tool}]=\\left(\n",
    "\\begin{array}{c|c} \n",
    "  ^0R_{tool} & 0 \\\\ \n",
    "  \\hline \n",
    "  0 & ^0R_{tool}\n",
    "\\end{array} \n",
    "\\right)\\vphantom{.}^{0}\\xi_{tool}$\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "With spatial Jacobian:        [ 0.75432431  0.82713012 -0.07730315  1.15255396 -0.40909473  0.78040047]\n",
      "With adjoint * body Jacobian: [ 0.75432431  0.82713012 -0.07730314  1.15255396 -0.40909473  0.78040047]\n",
      "o_Jtool3 from body Jacobian:  [ 0.79531852 -0.4021949  -0.78227251]\n",
      "Local-world-aligned Jacobian: [ 0.79525512 -0.40234713 -0.78225868]\n"
     ]
    }
   ],
   "source": [
    "# 0_w_tool and tool_w_tool\n",
    "tool_Jtool = pin.computeFrameJacobian(robot.model,robot.data,q,IDX_TOOL, pin.LOCAL)\n",
    "o_Jtool = pin.computeFrameJacobian(robot.model,robot.data,q,IDX_TOOL, pin.WORLD)\n",
    "print(f\"With spatial Jacobian:        {o_Jtool @ u}\")\n",
    "print(f\"With adjoint * body Jacobian: {oMtool.action @ (tool_Jtool @ u)}\")\n",
    "\n",
    "# 0_v_tool and tool_w_tool\n",
    "o_Jtool3 = oMtool.rotation @ tool_Jtool[:3, :]\n",
    "new_o_Jtool3 = pin.computeFrameJacobian(robot.model,robot.data,q,IDX_TOOL, pin.LOCAL_WORLD_ALIGNED)[:3,:]\n",
    "print(f\"o_Jtool3 from body Jacobian:  {o_Jtool3 @ u}\")\n",
    "print(f\"Local-world-aligned Jacobian: {new_o_Jtool3 @ u}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can also check out [this video](https://www.youtube.com/watch?v=MLFtHLTprE4) for more illustrations on these three frame options."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Inverse kinematics for the moving the robot effector\n",
    "We will first move only the robot end effector, to reach a target defined by a frame F_goal.\n",
    "\n",
    "## 机器人末端执行器的逆运动学\n",
    "我们首先将仅移动机器人末端执行器，以到达由框架F_goal定义的目标。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 43,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Goal placement, and integration in the viewer of the goal.\n",
    "oMgoal = pin.SE3(pin.Quaternion(-0.5, 0.58, -0.39, 0.52).normalized().matrix(), np.array([1.2, .4, .7]))\n",
    "viz.addBox('goal', [.1,.1,.1], [ .1,.1,.5, .6])\n",
    "viz.applyConfiguration('goal',oMgoal)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Position the effector (3D)\n",
    "\n",
    "It is time to write your first control law. Write a for loop to simulate the control cycles of the robot. At each control cycle, you should:\n",
    "\n",
    "* compute the Jacobian 3D in the world frame `o_Jtool3`\n",
    "* compute the vector from the tool to the goal, expressed in world frame: `o_TG = o_goal - o_tool`\n",
    "* compute the control law as `vq = pinv(o_Jtool3) @ o_TG`\n",
    "* integrate this during a duration `DT` to get to a new configuration `q`.\n",
    "\n",
    "You might want to start from the following initial configuration, or from any random one.\n",
    "\n",
    "Note that [`numpy.linalg.pinv`](https://numpy.org/doc/stable/reference/generated/numpy.linalg.pinv.html) in `pinv(J)` computes $J^+$, the Moore-Penrose pseudo-inverse of $J$. Precisely, for any vector $e$, $J^+ e$ is the solution of the linear least square problem:\n",
    "$$\\min_x \\|Jx - e\\|_2^2.$$ (If $J$ is not full rank and there are multiple such solutions, it corresponds to the one of minimum L2 norm $\\| x \\|$.)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 44,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Robot initial configuration\n",
    "q0 = np.array([ 0.  ,  0.  ,  1.  ,  0.  ,  0.18,  1.37, -0.24, -0.98,  0.98,\n",
    "                0.  ,  0.  ,  0.  ,  0.  , -0.13,  0.  ,  0.  ,  0.  ,  0.  ])\n",
    "DT = 1e-2  # seconds"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It is a good idea to store the values of the error between tool and goal (`o_TG`), to plot them later. For that, simply append each `o_TG` computed at every control cycle to a list:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = q0.copy()\n",
    "herr = [] # Log the value of the error between tool and goal.\n",
    "\n",
    "for i in range(500):  # Integrate over 2 second of robot life\n",
    "    \n",
    "    # REPLACE WITH YOUR CODE\n",
    "    o_TG = np.zeros(3)    \n",
    "    q = q0    \n",
    "\n",
    "    viz.display(q)\n",
    "    time.sleep(0.1)\n",
    "\n",
    "    herr.append(o_TG) \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And the solution if you need it"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 49,
   "metadata": {},
   "outputs": [
    {
     "ename": "ZMQError",
     "evalue": "Operation cannot be accomplished in current state",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mZMQError\u001b[0m                                  Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[49], line 25\u001b[0m\n\u001b[1;32m     22\u001b[0m vq \u001b[38;5;241m=\u001b[39m \u001b[38;5;241m-\u001b[39mpinv(o_Jtool3)\u001b[38;5;129m@o_TG\u001b[39m\n\u001b[1;32m     24\u001b[0m q \u001b[38;5;241m=\u001b[39m pin\u001b[38;5;241m.\u001b[39mintegrate(robot\u001b[38;5;241m.\u001b[39mmodel,q, vq \u001b[38;5;241m*\u001b[39m DT)\n\u001b[0;32m---> 25\u001b[0m \u001b[43mviz\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mdisplay\u001b[49m\u001b[43m(\u001b[49m\u001b[43mq\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m     26\u001b[0m time\u001b[38;5;241m.\u001b[39msleep(\u001b[38;5;241m1e-3\u001b[39m)\n\u001b[1;32m     28\u001b[0m herr\u001b[38;5;241m.\u001b[39mappend(o_TG) \n",
      "File \u001b[0;32m~/miniconda3/envs/robotics_course/lib/python3.10/site-packages/pinocchio/visualize/meshcat_visualizer.py:539\u001b[0m, in \u001b[0;36mMeshcatVisualizer.display\u001b[0;34m(self, q)\u001b[0m\n\u001b[1;32m    536\u001b[0m     \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mupdatePlacements(pin\u001b[38;5;241m.\u001b[39mGeometryType\u001b[38;5;241m.\u001b[39mCOLLISION)\n\u001b[1;32m    538\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mdisplay_visuals:\n\u001b[0;32m--> 539\u001b[0m     \u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mupdatePlacements\u001b[49m\u001b[43m(\u001b[49m\u001b[43mpin\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mGeometryType\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mVISUAL\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m    541\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mdisplay_frames:\n\u001b[1;32m    542\u001b[0m     \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mupdateFrames()\n",
      "File \u001b[0;32m~/miniconda3/envs/robotics_course/lib/python3.10/site-packages/pinocchio/visualize/meshcat_visualizer.py:566\u001b[0m, in \u001b[0;36mMeshcatVisualizer.updatePlacements\u001b[0;34m(self, geometry_type)\u001b[0m\n\u001b[1;32m    563\u001b[0m         T \u001b[38;5;241m=\u001b[39m M\u001b[38;5;241m.\u001b[39mhomogeneous\n\u001b[1;32m    565\u001b[0m     \u001b[38;5;66;03m# Update viewer configuration.\u001b[39;00m\n\u001b[0;32m--> 566\u001b[0m     \u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mviewer\u001b[49m\u001b[43m[\u001b[49m\u001b[43mvisual_name\u001b[49m\u001b[43m]\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mset_transform\u001b[49m\u001b[43m(\u001b[49m\u001b[43mT\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m    568\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m visual \u001b[38;5;129;01min\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mstatic_objects:\n\u001b[1;32m    569\u001b[0m     visual_name \u001b[38;5;241m=\u001b[39m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mgetViewerNodeName(visual, pin\u001b[38;5;241m.\u001b[39mGeometryType\u001b[38;5;241m.\u001b[39mVISUAL)\n",
      "File \u001b[0;32m~/miniconda3/envs/robotics_course/lib/python3.10/site-packages/meshcat/visualizer.py:152\u001b[0m, in \u001b[0;36mVisualizer.set_transform\u001b[0;34m(self, matrix)\u001b[0m\n\u001b[1;32m    151\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21mset_transform\u001b[39m(\u001b[38;5;28mself\u001b[39m, matrix\u001b[38;5;241m=\u001b[39mnp\u001b[38;5;241m.\u001b[39meye(\u001b[38;5;241m4\u001b[39m)):\n\u001b[0;32m--> 152\u001b[0m     \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mwindow\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msend\u001b[49m\u001b[43m(\u001b[49m\u001b[43mSetTransform\u001b[49m\u001b[43m(\u001b[49m\u001b[43mmatrix\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mpath\u001b[49m\u001b[43m)\u001b[49m\u001b[43m)\u001b[49m\n",
      "File \u001b[0;32m~/miniconda3/envs/robotics_course/lib/python3.10/site-packages/meshcat/visualizer.py:58\u001b[0m, in \u001b[0;36mViewerWindow.send\u001b[0;34m(self, command)\u001b[0m\n\u001b[1;32m     56\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21msend\u001b[39m(\u001b[38;5;28mself\u001b[39m, command):\n\u001b[1;32m     57\u001b[0m     cmd_data \u001b[38;5;241m=\u001b[39m command\u001b[38;5;241m.\u001b[39mlower()\n\u001b[0;32m---> 58\u001b[0m     \u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mzmq_socket\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msend_multipart\u001b[49m\u001b[43m(\u001b[49m\u001b[43m[\u001b[49m\n\u001b[1;32m     59\u001b[0m \u001b[43m        \u001b[49m\u001b[43mcmd_data\u001b[49m\u001b[43m[\u001b[49m\u001b[38;5;124;43m\"\u001b[39;49m\u001b[38;5;124;43mtype\u001b[39;49m\u001b[38;5;124;43m\"\u001b[39;49m\u001b[43m]\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mencode\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;124;43m\"\u001b[39;49m\u001b[38;5;124;43mutf-8\u001b[39;49m\u001b[38;5;124;43m\"\u001b[39;49m\u001b[43m)\u001b[49m\u001b[43m,\u001b[49m\n\u001b[1;32m     60\u001b[0m \u001b[43m        \u001b[49m\u001b[43mcmd_data\u001b[49m\u001b[43m[\u001b[49m\u001b[38;5;124;43m\"\u001b[39;49m\u001b[38;5;124;43mpath\u001b[39;49m\u001b[38;5;124;43m\"\u001b[39;49m\u001b[43m]\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mencode\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;124;43m\"\u001b[39;49m\u001b[38;5;124;43mutf-8\u001b[39;49m\u001b[38;5;124;43m\"\u001b[39;49m\u001b[43m)\u001b[49m\u001b[43m,\u001b[49m\n\u001b[1;32m     61\u001b[0m \u001b[43m        \u001b[49m\u001b[43mumsgpack\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mpackb\u001b[49m\u001b[43m(\u001b[49m\u001b[43mcmd_data\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m     62\u001b[0m \u001b[43m    \u001b[49m\u001b[43m]\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m     63\u001b[0m     \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mzmq_socket\u001b[38;5;241m.\u001b[39mrecv()\n",
      "File \u001b[0;32m~/miniconda3/envs/robotics_course/lib/python3.10/site-packages/zmq/sugar/socket.py:749\u001b[0m, in \u001b[0;36mSocket.send_multipart\u001b[0;34m(self, msg_parts, flags, copy, track, **kwargs)\u001b[0m\n\u001b[1;32m    745\u001b[0m         \u001b[38;5;28;01mraise\u001b[39;00m \u001b[38;5;167;01mTypeError\u001b[39;00m(\n\u001b[1;32m    746\u001b[0m             \u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mFrame \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mi\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m (\u001b[39m\u001b[38;5;132;01m{\u001b[39;00mrmsg\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m) does not support the buffer interface.\u001b[39m\u001b[38;5;124m\"\u001b[39m\n\u001b[1;32m    747\u001b[0m         )\n\u001b[1;32m    748\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m msg \u001b[38;5;129;01min\u001b[39;00m msg_parts[:\u001b[38;5;241m-\u001b[39m\u001b[38;5;241m1\u001b[39m]:\n\u001b[0;32m--> 749\u001b[0m     \u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msend\u001b[49m\u001b[43m(\u001b[49m\u001b[43mmsg\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mzmq\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mSNDMORE\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m|\u001b[39;49m\u001b[43m \u001b[49m\u001b[43mflags\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mcopy\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mcopy\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mtrack\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mtrack\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m    750\u001b[0m \u001b[38;5;66;03m# Send the last part without the extra SNDMORE flag.\u001b[39;00m\n\u001b[1;32m    751\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39msend(msg_parts[\u001b[38;5;241m-\u001b[39m\u001b[38;5;241m1\u001b[39m], flags, copy\u001b[38;5;241m=\u001b[39mcopy, track\u001b[38;5;241m=\u001b[39mtrack)\n",
      "File \u001b[0;32m~/miniconda3/envs/robotics_course/lib/python3.10/site-packages/zmq/sugar/socket.py:698\u001b[0m, in \u001b[0;36mSocket.send\u001b[0;34m(self, data, flags, copy, track, routing_id, group)\u001b[0m\n\u001b[1;32m    691\u001b[0m         data \u001b[38;5;241m=\u001b[39m zmq\u001b[38;5;241m.\u001b[39mFrame(\n\u001b[1;32m    692\u001b[0m             data,\n\u001b[1;32m    693\u001b[0m             track\u001b[38;5;241m=\u001b[39mtrack,\n\u001b[1;32m    694\u001b[0m             copy\u001b[38;5;241m=\u001b[39mcopy \u001b[38;5;129;01mor\u001b[39;00m \u001b[38;5;28;01mNone\u001b[39;00m,\n\u001b[1;32m    695\u001b[0m             copy_threshold\u001b[38;5;241m=\u001b[39m\u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mcopy_threshold,\n\u001b[1;32m    696\u001b[0m         )\n\u001b[1;32m    697\u001b[0m     data\u001b[38;5;241m.\u001b[39mgroup \u001b[38;5;241m=\u001b[39m group\n\u001b[0;32m--> 698\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[38;5;28;43msuper\u001b[39;49m\u001b[43m(\u001b[49m\u001b[43m)\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msend\u001b[49m\u001b[43m(\u001b[49m\u001b[43mdata\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mflags\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mflags\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mcopy\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mcopy\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mtrack\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mtrack\u001b[49m\u001b[43m)\u001b[49m\n",
      "File \u001b[0;32m_zmq.py:1081\u001b[0m, in \u001b[0;36mzmq.backend.cython._zmq.Socket.send\u001b[0;34m()\u001b[0m\n",
      "File \u001b[0;32m_zmq.py:1129\u001b[0m, in \u001b[0;36mzmq.backend.cython._zmq.Socket.send\u001b[0;34m()\u001b[0m\n",
      "File \u001b[0;32m_zmq.py:1402\u001b[0m, in \u001b[0;36mzmq.backend.cython._zmq._send_copy\u001b[0;34m()\u001b[0m\n",
      "File \u001b[0;32m_zmq.py:1397\u001b[0m, in \u001b[0;36mzmq.backend.cython._zmq._send_copy\u001b[0;34m()\u001b[0m\n",
      "File \u001b[0;32m_zmq.py:180\u001b[0m, in \u001b[0;36mzmq.backend.cython._zmq._check_rc\u001b[0;34m()\u001b[0m\n",
      "\u001b[0;31mZMQError\u001b[0m: Operation cannot be accomplished in current state"
     ]
    }
   ],
   "source": [
    "# %do_not_load tp3/generated/inverse_kinematics_3d_loop\n",
    "\n",
    "q = q0.copy()\n",
    "herr = [] # Log the value of the error between tool and goal.\n",
    "# Loop on an inverse kinematics for 200 iterations.\n",
    "for i in range(500):  # Integrate over 2 second of robot life\n",
    "\n",
    "    # Run the algorithms that outputs values in robot.data\n",
    "    pin.framesForwardKinematics(robot.model,robot.data,q)\n",
    "    pin.computeJointJacobians(robot.model,robot.data,q)\n",
    "\n",
    "    # Placement from world frame o to frame f oMtool\n",
    "    oMtool = robot.data.oMf[IDX_TOOL]\n",
    "\n",
    "    # 3D jacobian in world frame\n",
    "    o_Jtool3 = pin.computeFrameJacobian(robot.model,robot.data,q,IDX_TOOL,pin.LOCAL_WORLD_ALIGNED)[:3,:]\n",
    "\n",
    "    # vector from tool to goal, in world frame\n",
    "    o_TG = oMtool.translation-oMgoal.translation\n",
    "    \n",
    "    # Control law by least square\n",
    "    vq = -pinv(o_Jtool3)@o_TG\n",
    "\n",
    "    q = pin.integrate(robot.model,q, vq * DT)\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-3)\n",
    "\n",
    "    herr.append(o_TG) \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It is interesting to plot the behavior of the robot. If the error at each iteration has been stored as a list of 3x1 matrices, the following code plots it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 50,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 640x480 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "plt.plot(herr)\n",
    "plt.xlabel('control cycle (iter)')\n",
    "plt.ylabel('error (m)');"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can observe that each component of the error converges to zero following an exponential trajetory. The convergence is asymptotic. To fasten the convergence, increase the gain of the control law ($v_q = - \\lambda J^+ e$), where the gain $\\lambda$ was implicitly one so far."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Place the end effector (6D)\n",
    "The previous control law brings the center of the effector toward the center of the goal frame. However, it does not control the orientation of the end effector: the axes of the two frames $F_{tool}$ and $F_{goal}$ do not converge.\n",
    "\n",
    "We now want to modify the control law to take into account both tool translation and tool orientation. For that, let us compute the error in $SE(3)$. We will use the full ${}^{0} J_{tool}$ rather than ${}^o J_{tool3}$, and compute the error vector as the generalized difference between the tool frame and the goal frame:\n",
    "\n",
    "$$^{tool}M_{goal} = \\vphantom{.}^{0}M_{tool}^{-1} \\vphantom{.}^{0}M_{goal}$$\n",
    "$$^{tool}\\xi_{goal} = \\vphantom{.}^{0}M_{tool} \\ominus_{SE(3)} \\vphantom{.}^{0}M_{tool} = \\log(^{tool}M_{goal})$$\n",
    "where $^{tool}\\xi_{goal}$ is a twist in the local frame."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 51,
   "metadata": {},
   "outputs": [],
   "source": [
    "toolMgoal = oMtool.inverse() * oMgoal\n",
    "tool_w = pin.log(toolMgoal).vector"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This error `tool_w` is can be interpreted as the twist that should be applied during one second to displace the tool frame $F_{tool}$ (located at ${}^0 M_{tool}$) to the goal frame $F_{goal}$ (located at ${}^0 M_{goal}$).\n",
    "\n",
    "Implement a second control law, following the same pattern as with positions. At each control cycle, you should:\n",
    "\n",
    "* compute the displacement between $F_{tool}$ and $F_{goal}$, denoted by ``toolMgoal``\n",
    "* compute the error twist ``tool_w`` using the logarithm over $SE(3)$\n",
    "* compute the 6D Jacobian `tool_Jtool`\n",
    "* compute the control law `vq = pinv(J) @ nu`\n",
    "* integrate the robot velocity `vq` during `DT` to get to a new configuration `q`\n",
    "* log the error by storing it in a list `herr`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Your code"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And the solution:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'tp3' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[53], line 3\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[38;5;66;03m# %do_not_load tp3/generated/inverse_kinematics_6d_loop\u001b[39;00m\n\u001b[0;32m----> 3\u001b[0m \u001b[43mtp3\u001b[49m\u001b[38;5;241m/\u001b[39mgenerated\u001b[38;5;241m/\u001b[39minverse_kinematics_6d_loop\n",
      "\u001b[0;31mNameError\u001b[0m: name 'tp3' is not defined"
     ]
    }
   ],
   "source": [
    "# %do_not_load tp3/generated/inverse_kinematics_6d_loop\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The tool frame $F_{tool}$ converges toward the gooal frame $F_{goal}$: the center and the axes are finally aligned. The trajectory of the tool center is not a straight line, as the frame $F_{tool}$ follows a \"straight\" line, not in $\\mathbb{R}^3$ but in $SE(3)$.\n",
    "We can also plot the error (assuming that herr is a list of the 6D errors herr)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Text(0, 0.5, 'error (rad)')"
      ]
     },
     "execution_count": 29,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 640x480 with 2 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "plt.subplot(211)\n",
    "plt.plot([ e[:3] for e in herr])\n",
    "plt.xlabel('control cycle (iter)')\n",
    "plt.ylabel('error (m)')\n",
    "plt.subplot(212)\n",
    "plt.plot([ e[3:] for e in herr])\n",
    "plt.xlabel('control cycle (iter)')\n",
    "plt.ylabel('error (rad)')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Inverse kinematics with two tasks\n",
    "\n",
    "So far we controlled the robot with a single task, either the 3D position or 6D placement of the tool frame. Let's see how to take into account a second task: gaze control.\n",
    "\n",
    "#### Gaze control\n",
    "\n",
    "The robot has an additional frame, named $F_{gaze}$, attached to its head and located 40 cm in front of its cameras. The task will be to position (3D) the origin of this frame on an object of interest (a red ball)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7000/static/\n"
     ]
    }
   ],
   "source": [
    "robot = loadTiago(addGazeFrame=True)\n",
    "viz = MeshcatVisualizer(robot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 55,
   "metadata": {},
   "outputs": [],
   "source": [
    "IDX_GAZE = robot.model.getFrameId('framegaze')\n",
    "\n",
    "# Add a small ball as a visual target to be reached by the robot\n",
    "ball = np.array([ 1.2,0.5,1.1 ])\n",
    "viz.addSphere('ball', .05, [ .8,.1,.5, .8])\n",
    "viz.applyConfiguration('ball', list(ball)+[0,0,0,1])\n",
    "\n",
    "# Add the box again\n",
    "oMgoal = pin.SE3(pin.Quaternion(-0.5, 0.58, -0.39, 0.52).normalized().matrix(), np.array([1.2, .4, .7]))\n",
    "viz.addBox('goal', [.1,.1,.1], [ .1,.1,.5, .6])\n",
    "viz.applyConfiguration('goal', oMgoal)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 56,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "            <div style=\"height: 400px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
       "            <iframe src=\"http://127.0.0.1:7000/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "            </div>\n",
       "            "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 56,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "viz.display(q0)\n",
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Controlling this point can be done by achieving a simple variation of the control law for positioning (3D) the robot tool:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Your code"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is the solution if you need it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 59,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %do_not_load tp3/generated/control_head_gaze_loop\n",
    "\n",
    "q = q0.copy()\n",
    "herr = [] # Log the value of the error between gaze and ball.\n",
    "# Loop on an inverse kinematics for 200 iterations.\n",
    "for i in range(500):  # Integrate over 2 second of robot life\n",
    "\n",
    "    # Run the algorithms that outputs values in robot.data\n",
    "    pin.framesForwardKinematics(robot.model,robot.data,q)\n",
    "    pin.computeJointJacobians(robot.model,robot.data,q)\n",
    "\n",
    "    # Placement from world frame o to frame f oMgaze\n",
    "    oMgaze = robot.data.oMf[IDX_GAZE]\n",
    "\n",
    "    # 6D jacobian in local frame\n",
    "    o_Jgaze3 = pin.computeFrameJacobian(robot.model, robot.data, q, IDX_GAZE,pin.LOCAL_WORLD_ALIGNED)[:3,:]\n",
    "\n",
    "    # vector from gaze to ball, in world frame\n",
    "    o_GazeBall = oMgaze.translation-ball\n",
    "    \n",
    "    vq = -pinv(o_Jgaze3) @ o_GazeBall\n",
    "\n",
    "    q = pin.integrate(robot.model,q, vq * DT)\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-3)\n",
    "\n",
    "    herr.append(o_GazeBall) \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Multi-tasking\n",
    "\n",
    "We now have two tasks: $(e_1, J_1)$ for controlling the tool placement, and $(e_2, J_2)$ for controlling the gaze position. We can use the previous derivation to compute the optimal (in a least square sense) control for task 1:\n",
    "$$\n",
    "\\min_{x\\in \\mathbb{R}^n} \\|J_1 x - v_1^*\\|_2^2\n",
    "$$\n",
    "with solution:\n",
    "$$\n",
    "vq_1 = J_1^+ v_1^*\n",
    "$$\n",
    "\n",
    "We may now add to $vq_1$ any vector that lies is in the nullspace of $J_1$ to perform the second task. We thus search for $vq_2 = vq_1 + dvq_2$, where $dvq_2$ gives an optimal control for task 2 while constraining $dvq_2$ is in the nullspace of $J_1$.\n",
    "$$\n",
    "\\min_{x\\in Ker(J_1)} \\|J_2 (vq_1 + x) - v_2^*\\|_2^2\n",
    "$$\n",
    "The orthogonal nullspace projector of $J_1$ can be computed using the pseudoinverse:\n",
    "$$\n",
    "P_1 = I_{nv} - J_1^+ J_1\n",
    "$$\n",
    "Instead we can solve the problem:\n",
    "$$\n",
    "\\min_{x\\mathbb{R}^n} \\|J_2 (vq_1 + P_1 x) - v_2^*\\|_2^2\n",
    "$$\n",
    "with solution:\n",
    "$$\n",
    "dvq_2 = (J_2 P_1)^+ (v_2^* - J_2 vq_1)\n",
    "$$\n",
    "Finally, the control law to perform task 1 and task 2 in the nullspace of task 1 is:\n",
    "$$\n",
    "vq_2 = J_1^+ v_1^* + P_1 (J_2 P_1)^+ ( v_2^* - J_2 J_1^+ v_1^*)\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = q0.copy()\n",
    "herr = [] # Log the value of the error between tool and goal\n",
    "herr2 = [] # Log the value of the error between gaze and ball\n",
    "\n",
    "# Your code"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is the solution:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 60,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %do_not_load tp3/generated/control_head_multi\n",
    "\n",
    "q = q0.copy()\n",
    "herr = [] # Log the value of the error between tool and goal.\n",
    "herr2 = [] # Log the value of the error between gaze and ball.\n",
    "# Loop on an inverse kinematics for 200 iterations.\n",
    "for i in range(500):  # Integrate over 2 second of robot life\n",
    "\n",
    "    # Run the algorithms that outputs values in robot.data\n",
    "    pin.framesForwardKinematics(robot.model,robot.data,q)\n",
    "    pin.computeJointJacobians(robot.model,robot.data,q)\n",
    "\n",
    "    # Tool task\n",
    "    oMtool = robot.data.oMf[IDX_TOOL]\n",
    "    tool_Jtool = pin.computeFrameJacobian(robot.model,robot.data,q,IDX_TOOL,pin.LOCAL)\n",
    "    tool_nu = pin.log(oMtool.inverse()*oMgoal).vector\n",
    "\n",
    "    # Gaze task\n",
    "    oMgaze = robot.data.oMf[IDX_GAZE]\n",
    "    o_Jgaze3 = pin.computeFrameJacobian(robot.model, robot.data, q, IDX_GAZE,pin.LOCAL_WORLD_ALIGNED)[:3,:]\n",
    "    o_GazeBall = oMgaze.translation-ball\n",
    "\n",
    "    vq = pinv(tool_Jtool) @ tool_nu\n",
    "    Ptool = np.eye(robot.nv)-pinv(tool_Jtool) @ tool_Jtool\n",
    "    vq += Ptool @ pinv(o_Jgaze3 @ Ptool) @ (-o_GazeBall - o_Jgaze3 @ vq)\n",
    "\n",
    "    q = pin.integrate(robot.model,q, vq * DT)\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-3)\n",
    "\n",
    "    herr.append(o_TG)\n",
    "    herr2.append(o_GazeBall) \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Text(0, 0.5, 'error (rad)')"
      ]
     },
     "execution_count": 37,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 640x480 with 3 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "plt.subplot(311)\n",
    "plt.plot([ e[:3] for e in herr])\n",
    "plt.xlabel('control cycle (iter)')\n",
    "plt.ylabel('error (m)')\n",
    "plt.subplot(312)\n",
    "plt.plot([ e[3:] for e in herr])\n",
    "plt.xlabel('control cycle (iter)')\n",
    "plt.ylabel('error (rad)');\n",
    "plt.subplot(313)\n",
    "plt.plot([ e for e in herr2])\n",
    "plt.xlabel('control cycle (iter)')\n",
    "plt.ylabel('error (rad)')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Extension to three tasks"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that the previous reasoning can be extended recursively and we have more than 2 tasks.\n",
    "\n",
    "A third task can be implemented as well by computing the nullspace of the two first tasks:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 38,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'Ptool' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[38], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m Pgaze \u001b[38;5;241m=\u001b[39m \u001b[43mPtool\u001b[49m \u001b[38;5;241m-\u001b[39m pinv(o_Jgaze3 \u001b[38;5;241m@\u001b[39m Ptool) \u001b[38;5;241m@\u001b[39m o_Jgaze3 \u001b[38;5;241m@\u001b[39m Ptool\n",
      "\u001b[0;31mNameError\u001b[0m: name 'Ptool' is not defined"
     ]
    }
   ],
   "source": [
    "Pgaze = Ptool - pinv(o_Jgaze3 @ Ptool) @ o_Jgaze3 @ Ptool"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Load an extra cube in the viewer to figure a table. First, control the robot hand to reach an arbitrary point on the table (don't worry about collisions). Then, implement a control law to control three tasks:\n",
    "* The tool frame should be kept on the table (only the vertical component of the error matters, select the third row of the Jacobian and error accordingly).\n",
    "* The gaze should reach the position of a ball positionned on the table.\n",
    "* The center of the mobile base frame should reach a given goal on the floor. For this task, only the horizontal components (x- and y-) of the task matter, select only the first two rows of the Jacobian and error accordingly."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Your code"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Congratulations! You just implemented a local optimal controller that can solve multiple tasks at once."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "robotics_course",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.17"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
